/*
 * @Description: 显示模块节点
 * @Author: Sang Hao
 * @Date: 2021-10-21 16:03:51
 * @LastEditTime: 2021-11-12 10:08:30
 * @LastEditors: Sang Hao
 */

#include <ros/ros.h>
#include "glog/logging.h"

#include <lidar_slam/saveMap.h>
#include <lidar_slam/segMap.h>
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/viewer/viewer_flow.hpp"

using namespace lidar_slam;

std::shared_ptr<ViewerFlow> _viewer_flow_ptr;
bool _need_save_map = false;

bool _need_seg_map = false;

/* 这种服务回调函数感觉不好，只更改了bool值，就返回succeed了，但并不知道这个bool值能否真的实现需需要的功能 */
bool save_map_callback(saveMap::Request& request, saveMap::Response& response) {
	_need_save_map = true;
	response.succeed = true;
	return response.succeed;

}

bool seg_map_callback(segMap::Request& request, segMap::Response& response) {
	_need_seg_map = true;
	response.succeed = true;
	return response.succeed;
}

int main(int argc, char* argv[]) {
	google::InitGoogleLogging(argv[0]);
	FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
	FLAGS_alsologtostderr = 1;

	ros::init(argc, argv, "viewer_node");
	ros::NodeHandle nh;

	ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback);
	ros::ServiceServer service_seg_map = nh.advertiseService("seg_map", seg_map_callback);
	std::shared_ptr<ViewerFlow> _viewer_flow_ptr = std::make_shared<ViewerFlow>(nh);
	
	ros::Rate rate(100);
	while (ros::ok()) {
		ros::spinOnce();

		_viewer_flow_ptr->Run();
		if (_need_save_map) {
			_need_save_map = false;
			_viewer_flow_ptr->SaveMap();
		}

		if (_need_seg_map) {
			_need_seg_map = false;
			_viewer_flow_ptr->PublishGlobalSegData();
		}

		rate.sleep();
	}

	return 0;
}